/*   Copyright (C) 2008  Jeroen v. Schagen & Youri Lima
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

using System;

namespace Engine.Math
{
    /// <summary>
    ///     Represents a matrix in a four-dimensional space.
    /// 
    ///     In mathematics, a matrix (plural matrices) is a rectangular table of 
    ///     elements (or entries), which may be numbers or, more generally, any 
    ///     abstract quantities that can be added and multiplied.
    /// 
    ///     Matrices are used to describe linear equations, keep track of the 
    ///     coefficients of linear transformations and to record data that depend 
    ///     on multiple parameters. Matrices are described by the field of matrix theory. 
    /// 
    ///     Matrices can be added, multiplied, and decomposed in various ways, 
    ///     which also makes them a key concept in the field of linear algebra.
    /// </summary>
    public class Matrix4
    {
        public float[,] m = new float[4, 4];

        # region Constant

        /// <summary>
        ///     The representation of an empty four-dimensional matrix.
        /// </summary>
        public static Matrix4 ZERO
        {
            get
            {
                return new Matrix4( 0, 0, 0, 0,
                                    0, 0, 0, 0,
                                    0, 0, 0, 0,
                                    0, 0, 0, 0 );
            }
        }

        /// <summary>
        ///     The representation of a four-dimensional identity matrix.
        /// 
        ///     In linear algebra, the identity matrix or unit matrix of 
        ///     size n is the n-by-n square matrix with ones on the main 
        ///     diagonal and zeros elsewhere.
        /// </summary>
        public static Matrix4 IDENTITY
        {
            get
            {
                return new Matrix4( 1, 0, 0, 0,
                                    0, 1, 0, 0,
                                    0, 0, 1, 0,
                                    0, 0, 0, 1 );
            }
        }

        # endregion


        # region Constructor

        /// <summary>
        ///     Default no-arg constructor.
        /// </summary>
        public Matrix4()
        { }

        /// <summary>
        ///     Construct matrix using two-dimensional array.
        /// </summary>
        /// 
        /// <param name="m"></param>
        public Matrix4(float[,] m)
        {
            this.m = m;
        }

        /// <summary>
        ///     Construct matrix using matrix4 object.
        /// </summary>
        /// 
        /// <param name="matrix"></param>
        public Matrix4(Matrix4 matrix)
        {
            this.m = matrix.m;
        }

        /// <summary>
        ///     Construct matrix using three vector objects.
        /// </summary>
        /// 
        /// <param name="x">X axis</param>
        /// <param name="y">Y axis</param>
        /// <param name="z">Z axis</param>
        public Matrix4(Vector3 x, Vector3 y, Vector3 z) : this(Matrix4.IDENTITY)
        {
            m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x;
            m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y;
            m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z;
        }

        /// <summary>
        ///     Construct matrix using sixteen float values.
        /// </summary>
        public Matrix4(float m00, float m01, float m02, float m03,
                       float m10, float m11, float m12, float m13,
                       float m20, float m21, float m22, float m23,
                       float m30, float m31, float m32, float m33)
        {
            m[0, 0] = m00; m[0, 1] = m01; m[0, 2] = m02; m[0, 3] = m03;
            m[1, 0] = m10; m[1, 1] = m11; m[1, 2] = m12; m[1, 3] = m13;
            m[2, 0] = m20; m[2, 1] = m21; m[2, 2] = m22; m[2, 3] = m23;
            m[3, 0] = m30; m[3, 1] = m31; m[3, 2] = m32; m[3, 3] = m33;
        }

        # endregion


        # region Function

        /// <summary>
        ///     A determinant is a scalar number which is calculated from a matrix. 
        ///     This number can determine whether a set of linear equations 
        ///     are solvable, in other words whether the matrix can be inverted.
        /// </summary>
        public float Determinant
        {
            get
            {
                return m[0, 3] * m[1, 2] * m[2, 1] * m[3, 0] - m[0, 2] * m[1, 3] * m[2, 1] * m[3, 0] -
                       m[0, 3] * m[1, 1] * m[2, 2] * m[3, 0] + m[0, 1] * m[1, 3] * m[2, 2] * m[3, 0] +
                       m[0, 2] * m[1, 1] * m[2, 3] * m[3, 0] - m[0, 1] * m[1, 2] * m[2, 3] * m[3, 0] -
                       m[0, 3] * m[1, 2] * m[2, 0] * m[3, 1] + m[0, 2] * m[1, 3] * m[2, 0] * m[3, 1] +
                       m[0, 3] * m[1, 0] * m[2, 2] * m[3, 1] - m[0, 0] * m[1, 3] * m[2, 2] * m[3, 1] -
                       m[0, 2] * m[1, 0] * m[2, 3] * m[3, 1] + m[0, 0] * m[1, 2] * m[2, 3] * m[3, 1] +
                       m[0, 3] * m[1, 1] * m[2, 0] * m[3, 2] - m[0, 1] * m[1, 3] * m[2, 0] * m[3, 2] -
                       m[0, 3] * m[1, 0] * m[2, 1] * m[3, 2] + m[0, 0] * m[1, 3] * m[2, 1] * m[3 ,2] +
                       m[0, 1] * m[1, 0] * m[2, 3] * m[3, 2] - m[0, 0] * m[1, 1] * m[2, 3] * m[3, 2] -
                       m[0, 2] * m[1, 1] * m[2, 0] * m[3, 3] + m[0, 1] * m[1, 2] * m[2, 0] * m[3, 3] +
                       m[0, 2] * m[1, 0] * m[2, 1] * m[3, 3] - m[0, 0] * m[1, 2] * m[2, 1] * m[3, 3] -
                       m[0, 1] * m[1, 0] * m[2, 2] * m[3, 3] + m[0, 0] * m[1, 1] * m[2, 2] * m[3, 3];
            }
        }

        /// <summary>
        ///     Create a four-dimensional matrix, representing a certain
        ///     orientation based on a quaternion object.
        /// </summary>
        /// 
        /// <param name="q">Rotation</param>
        /// <returns>Matrix</returns>
        public static Matrix4 FromRotation(Quaternion q)
        {
            Matrix4 r = Matrix4.IDENTITY;

            r.m[0, 0] = 1 - 2 * (q.y * q.y) - 2 * (q.z * q.z);
            r.m[0, 1] = 2 * (q.x * q.y) - 2 * (q.z * q.w);
            r.m[0, 2] = 2 * (q.x * q.z) + 2 * (q.y * q.w);

            r.m[1, 0] = 2 * (q.x * q.y) + 2 * (q.z * q.w);
            r.m[1, 1] = 1 - 2 * (q.x * q.x) - 2 * (q.z * q.z);
            r.m[1, 2] = 2 * (q.y * q.z) - 2 * (q.x * q.w);

            r.m[2, 0] = 2 * (q.w * q.z) - 2 * (q.y * q.w);
            r.m[2, 1] = 2 * (q.y * q.z) + 2 * (q.x * q.w);
            r.m[2, 2] = 1 - 2 * (q.x * q.x) - 2 * (q.y * q.y);

            return r;
        }

        /// <summary>
        ///     Create a four-dimensional matrix, representing a certain
        ///     scale based on a three dimensional vector object.
        /// </summary>
        /// 
        /// <param name="v">Scale</param>
        /// <returns>Matrix</returns>
        public static Matrix4 FromScale(Vector3 v)
        {
            Matrix4 r = Matrix4.IDENTITY;

            // Construct scale matrix
            r.m[0, 0] = v.x;
            r.m[1, 1] = v.y;
            r.m[2, 2] = v.z;

            return r;
        }

        /// <summary>
        ///     Create a four-dimensional matrix, representing a certain
        ///     translation based on a three dimensional vector object.
        /// </summary>
        /// 
        /// <param name="v">Translation</param>
        /// <returns>Matrix</returns>
        public static Matrix4 FromTranslation(Vector3 v)
        {
            Matrix4 r = Matrix4.IDENTITY;

            // Construct translation matrix
            r.m[0, 3] = v.x;
            r.m[1, 3] = v.y;
            r.m[2, 3] = v.z;

            return r;
        }

        /// <summary>
        ///     Calculates the inverse of this matrix.
        /// 
        ///     An inverse is the opposite of something. This word and 
        ///     its derivatives are used greatly in mathematics.
        /// </summary>
        public Matrix4 Inverse
        {
            get
            {
                Matrix4 inverse = new Matrix4();
                Matrix4 matrix = new Matrix4(this);

                float v0 = matrix.m[2, 0] * matrix.m[3, 1] - matrix.m[2, 1] * matrix.m[3, 0];
                float v1 = matrix.m[2, 0] * matrix.m[3, 2] - matrix.m[2, 2] * matrix.m[3, 0];
                float v2 = matrix.m[2, 0] * matrix.m[3, 3] - matrix.m[2, 3] * matrix.m[3, 0];
                float v3 = matrix.m[2, 1] * matrix.m[3, 2] - matrix.m[2, 2] * matrix.m[3, 1];
                float v4 = matrix.m[2, 1] * matrix.m[3, 3] - matrix.m[2, 3] * matrix.m[3, 1];
                float v5 = matrix.m[2, 2] * matrix.m[3, 3] - matrix.m[2, 3] * matrix.m[3, 2];

                float t00 = +(v5 * matrix.m[1, 1] - v4 * matrix.m[1, 2] + v3 * matrix.m[1, 3]);
                float t10 = -(v5 * matrix.m[1, 0] - v2 * matrix.m[1, 2] + v1 * matrix.m[1, 3]);
                float t20 = +(v4 * matrix.m[1, 0] - v2 * matrix.m[1, 1] + v0 * matrix.m[1, 3]);
                float t30 = -(v3 * matrix.m[1, 0] - v1 * matrix.m[1, 1] + v0 * matrix.m[1, 2]);

                float invDet = 1 / (t00 * matrix.m[0, 0] + t10 * matrix.m[0, 1] + t20 * matrix.m[0, 2] + t30 * matrix.m[0, 3]);

                // First row
                inverse.m[0, 0] = t00 * invDet;
                inverse.m[1, 0] = t10 * invDet;
                inverse.m[2, 0] = t20 * invDet;
                inverse.m[3, 0] = t30 * invDet;

                // Second row
                inverse.m[0, 1] = -(v5 * matrix.m[0, 1] - v4 * matrix.m[0, 2] + v3 * matrix.m[0, 3]) * invDet;
                inverse.m[1, 1] = +(v5 * matrix.m[0, 0] - v2 * matrix.m[0, 2] + v1 * matrix.m[0, 3]) * invDet;
                inverse.m[2, 1] = -(v4 * matrix.m[0, 0] - v2 * matrix.m[0, 1] + v0 * matrix.m[0, 3]) * invDet;
                inverse.m[3, 1] = +(v3 * matrix.m[0, 0] - v1 * matrix.m[0, 1] + v0 * matrix.m[0, 2]) * invDet;

                v0 = matrix.m[1, 0] * matrix.m[3, 1] - matrix.m[1, 1] * matrix.m[3, 0];
                v1 = matrix.m[1, 0] * matrix.m[3, 2] - matrix.m[1, 2] * matrix.m[3, 0];
                v2 = matrix.m[1, 0] * matrix.m[3, 3] - matrix.m[1, 3] * matrix.m[3, 0];
                v3 = matrix.m[1, 1] * matrix.m[3, 2] - matrix.m[1, 2] * matrix.m[3, 1];
                v4 = matrix.m[1, 1] * matrix.m[3, 3] - matrix.m[1, 3] * matrix.m[3, 1];
                v5 = matrix.m[1, 2] * matrix.m[3, 3] - matrix.m[1, 3] * matrix.m[3, 2];

                // Third row
                inverse.m[0, 2] = +(v5 * matrix.m[0, 1] - v4 * matrix.m[0, 2] + v3 * matrix.m[0, 3]) * invDet;
                inverse.m[1, 2] = -(v5 * matrix.m[0, 0] - v2 * matrix.m[0, 2] + v1 * matrix.m[0, 3]) * invDet;
                inverse.m[2, 2] = +(v4 * matrix.m[0, 0] - v2 * matrix.m[0, 1] + v0 * matrix.m[0, 3]) * invDet;
                inverse.m[3, 2] = -(v3 * matrix.m[0, 0] - v1 * matrix.m[0, 1] + v0 * matrix.m[0, 2]) * invDet;

                v0 = matrix.m[2, 1] * matrix.m[1, 0] - matrix.m[2, 0] * matrix.m[1, 1];
                v1 = matrix.m[2, 2] * matrix.m[1, 0] - matrix.m[2, 0] * matrix.m[1, 2];
                v2 = matrix.m[2, 3] * matrix.m[1, 0] - matrix.m[2, 0] * matrix.m[1, 3];
                v3 = matrix.m[2, 2] * matrix.m[1, 1] - matrix.m[2, 1] * matrix.m[1, 2];
                v4 = matrix.m[2, 3] * matrix.m[1, 1] - matrix.m[2, 1] * matrix.m[1, 3];
                v5 = matrix.m[2, 3] * matrix.m[1, 2] - matrix.m[2, 2] * matrix.m[1, 3];

                // Fourth row
                inverse.m[0, 3] = -(v5 * matrix.m[0, 1] - v4 * matrix.m[0, 2] + v3 * matrix.m[0, 3]) * invDet;
                inverse.m[1, 3] = +(v5 * matrix.m[0, 0] - v2 * matrix.m[0, 2] + v1 * matrix.m[0, 3]) * invDet;
                inverse.m[2, 3] = -(v4 * matrix.m[0, 0] - v2 * matrix.m[0, 1] + v0 * matrix.m[0, 3]) * invDet;
                inverse.m[3, 3] = +(v3 * matrix.m[0, 0 ]- v1 * matrix.m[0, 1] + v0 * matrix.m[0, 2]) * invDet;

                return inverse;
            }
        }

        /// <summary>
        ///     Apply a rotation matrix to this matrix.
        /// </summary>
        /// 
        /// <param name="q"></param>
        public void Rotate(Quaternion q)
        {
            Matrix4 matrix = Matrix4.FromRotation(q);

            // Apply scaling
            m = (this * matrix).m;
        }

        /// <summary>
        ///     Apply a scalar matrix to this matrix.
        /// </summary>
        /// 
        /// <param name="v">Scaling</param>
        public void Scale(Vector3 v)
        {
            Matrix4 matrix = Matrix4.FromScale(v);

            // Apply scaling
            m = (this * matrix).m;
        }

        /// <summary>
        ///     Apply a translation matrix to this matrix.
        /// </summary>
        /// 
        /// <param name="v">Translation</param>
        public void Translate(Vector3 v)
        {
            Matrix4 matrix = Matrix4.FromTranslation(v);

            // Apply translation
            m = (this * matrix).m;
        }

        /// <summary>
        ///     Transpose the current matrix.
        /// </summary>
        public Matrix4 Transpose
        {
            get
            {
                return new Matrix4( m[0, 0], m[1, 0], m[2, 0], m[3, 0],
                                    m[0, 1], m[1, 1], m[2, 1], m[3, 1],
                                    m[0, 2], m[1, 2], m[2, 2], m[3, 2],
                                    m[0, 3], m[1, 3], m[2, 3], m[3, 3] );
            }
        }

        # endregion


        # region Operator

        public static Matrix4 operator +(Matrix4 matrix1, Matrix4 matrix2)
        {
            float[,] value = new float[4,4];

            for(int i = 0; i < 4; i++)
                for(int j = 0; j < 4; j++)
                    value[i, j] = matrix1.m[i, j] + matrix2.m[i, j];

            return new Matrix4(value);
        }

        public static Matrix4 operator -(Matrix4 matrix1, Matrix4 matrix2)
        {
            float[,] value = new float[4, 4];

            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    value[i, j] = matrix1.m[i, j] - matrix2.m[i, j];

            return new Matrix4(value);
        }

        public static Matrix4 operator *(Matrix4 matrix1, Matrix4 matrix2)
        {
            Matrix4 r = new Matrix4();

            r.m[0, 0] = matrix1.m[0, 0] * matrix2.m[0, 0] + matrix1.m[0, 1] * matrix2.m[1, 0] + matrix1.m[0, 2] * matrix2.m[2, 0] + matrix1.m[0, 3] * matrix2.m[3, 0];
            r.m[0, 1] = matrix1.m[0, 0] * matrix2.m[0, 1] + matrix1.m[0, 1] * matrix2.m[1, 1] + matrix1.m[0, 2] * matrix2.m[2, 1] + matrix1.m[0, 3] * matrix2.m[3, 1];
            r.m[0, 2] = matrix1.m[0, 0] * matrix2.m[0, 2] + matrix1.m[0, 1] * matrix2.m[1, 2] + matrix1.m[0, 2] * matrix2.m[2, 2] + matrix1.m[0, 3] * matrix2.m[3, 2];
            r.m[0, 3] = matrix1.m[0, 0] * matrix2.m[0, 3] + matrix1.m[0, 1] * matrix2.m[1, 3] + matrix1.m[0, 2] * matrix2.m[2, 3] + matrix1.m[0, 3] * matrix2.m[3, 3];

            r.m[1, 0] = matrix1.m[1, 0] * matrix2.m[0, 0] + matrix1.m[1, 1] * matrix2.m[1, 0] + matrix1.m[1, 2] * matrix2.m[2, 0] + matrix1.m[1, 3] * matrix2.m[3, 0];
            r.m[1, 1] = matrix1.m[1, 0] * matrix2.m[0, 1] + matrix1.m[1, 1] * matrix2.m[1, 1] + matrix1.m[1, 2] * matrix2.m[2, 1] + matrix1.m[1, 3] * matrix2.m[3, 1];
            r.m[1, 2] = matrix1.m[1, 0] * matrix2.m[0, 2] + matrix1.m[1, 1] * matrix2.m[1, 2] + matrix1.m[1, 2] * matrix2.m[2, 2] + matrix1.m[1, 3] * matrix2.m[3, 2];
            r.m[1, 3] = matrix1.m[1, 0] * matrix2.m[0, 3] + matrix1.m[1, 1] * matrix2.m[1, 3] + matrix1.m[1, 2] * matrix2.m[2, 3] + matrix1.m[1, 3] * matrix2.m[3, 3];

            r.m[2, 0] = matrix1.m[2, 0] * matrix2.m[0, 0] + matrix1.m[2, 1] * matrix2.m[1, 0] + matrix1.m[2, 2] * matrix2.m[2, 0] + matrix1.m[2, 3] * matrix2.m[3, 0];
            r.m[2, 1] = matrix1.m[2, 0] * matrix2.m[0, 1] + matrix1.m[2, 1] * matrix2.m[1, 1] + matrix1.m[2, 2] * matrix2.m[2, 1] + matrix1.m[2, 3] * matrix2.m[3, 1];
            r.m[2, 2] = matrix1.m[2, 0] * matrix2.m[0, 2] + matrix1.m[2, 1] * matrix2.m[1, 2] + matrix1.m[2, 2] * matrix2.m[2, 2] + matrix1.m[2, 3] * matrix2.m[3, 2];
            r.m[2, 3] = matrix1.m[2, 0] * matrix2.m[0, 3] + matrix1.m[2, 1] * matrix2.m[1, 3] + matrix1.m[2, 2] * matrix2.m[2, 3] + matrix1.m[2, 3] * matrix2.m[3, 3];

            r.m[3, 0] = matrix1.m[3, 0] * matrix2.m[0, 0] + matrix1.m[3, 1] * matrix2.m[1, 0] + matrix1.m[3, 2] * matrix2.m[2, 0] + matrix1.m[3, 3] * matrix2.m[3, 0];
            r.m[3, 1] = matrix1.m[3, 0] * matrix2.m[0, 1] + matrix1.m[3, 1] * matrix2.m[1, 1] + matrix1.m[3, 2] * matrix2.m[2, 1] + matrix1.m[3, 3] * matrix2.m[3, 1];
            r.m[3, 2] = matrix1.m[3, 0] * matrix2.m[0, 2] + matrix1.m[3, 1] * matrix2.m[1, 2] + matrix1.m[3, 2] * matrix2.m[2, 2] + matrix1.m[3, 3] * matrix2.m[3, 2];
            r.m[3, 3] = matrix1.m[3, 0] * matrix2.m[0, 3] + matrix1.m[3, 1] * matrix2.m[1, 3] + matrix1.m[3, 2] * matrix2.m[2, 3] + matrix1.m[3, 3] * matrix2.m[3, 3];

            return r;
        }

        # endregion


        # region Cross-class operator

        // Float
        public static Matrix4 operator *(Matrix4 matrix, float value)
        {
            // Apply value(s)
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    matrix.m[i, j] *= value;

            // Return new matrix
            return matrix;
        }

        /* Plane
        public static Plane operator *(Matrix4 matrix, Plane plane)
        {
            Plane r = new Plane();
			Matrix4 invTrans = matrix.Inverse.Transpose;

            r.normal = invTrans * plane.normal;
			r.d = (invTrans * plane.d) / r.normal.Normalise();

            return r;
        }*/

        // Vector3
        public static Vector3 operator *(Matrix4 matrix, Vector3 vector)
        {
            // Calculate w
            float w = 1.0f / ( matrix.m[3, 0] * vector.x + matrix.m[3, 1] * vector.y + matrix.m[3, 2] * vector.z + matrix.m[3, 3] );

            // Return new vector
            return new Vector3( ( matrix.m[0, 0] * vector.x + matrix.m[0, 1] * vector.y + matrix.m[0, 2] * vector.z + matrix.m[0, 3] ) * w,
                             ( matrix.m[1, 0] * vector.x + matrix.m[1, 1] * vector.y + matrix.m[1, 2] * vector.z + matrix.m[1, 3] ) * w,
                             ( matrix.m[2, 0] * vector.x + matrix.m[2, 1] * vector.y + matrix.m[2, 2] * vector.z + matrix.m[2, 3] ) * w );
        }

        # endregion

        public override string ToString()
        {
            string r = "";

            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    r += "m" + i + j + ": " + m[i, j] + ", ";

            return r;
        }
    }
}
